
#include <limits>
#include <typelib/operations.h>

namespace g3 {

/*
	This code is ported from Align's Boolean2 library; 
	May contain errors, because not all functions are used and tested so far
*/

template <typename T>
bool project_point_on_segment(const basic_vector<T> & p, 
	const basic_vector<T> & p0, const basic_vector<T> & p1, 
	bool & degenerate, T & h2, T & t)
{
	basic_vector<T> a = p - p0, b = p1 - p0;
	// Lengths of the segment
	T b2 = square(b);
	T eps = std::numeric_limits<T>::epsilon();
	if (degenerate = (b2 < eps * eps))
		return false;
	// Parameter of the projection
	t = scalar_product(a, b) / b2;
	// Distance to the projection point
	h2 = square(outer_product(a, b)) / b2;
	return t >= 0 && t <= 1;
}

template <typename T>
bool cross_segments(const basic_vector<T> & p0, const basic_vector<T> & p1, 
	const basic_vector<T> & q0, const basic_vector<T> & q1, 
	bool & collinear, T & h, T & s, T & t)
{
	/*
		Projection points: p = p0 + t * (p1 - p0), q = q0 + s * (q1 - q0) must
		satisfy the following equations:
			(p - q, p1 - p0) = 0
			(p - q, q1 - q0) = 0.
		We rewrite this as follows:
			A t - B s = D,
			B t - C s = E,
		where A = (p1 - p0)^2, B = (p1 - p0, q1 - q0), C = (q1 - q0)^2,
		D = - (p0 - q0, p1 - p0), E = - (p0 - q0, q1 - q0).
	*/
	basic_vector<T> pp = p1 - p0, qq = q1 - q0, pq = p0 - q0;

	T A = T(square(pp)), B = (pp * qq), C = square(qq),
		D = - (pq * pp), E = - (pq * qq);

	T det = B * B - A * C;
	T eps = std::numeric_limits<T>::epsilon();
	if (collinear = (- det < 2 * eps))
		return false;

	basic_vector<T> n = outer_product(pp, qq);

	h = scalar_product(pq, n) / sqrt(- det);

	s = (B * E - C * D) / det;
	t = (A * E - B * D) / det;

	return (s >= 0 && s <= 1) && (t >= 0 && t <= 1);
}

template <typename T>
bool project_point_on_triangle(const basic_vector<T> & p,
	const basic_vector<T> & q0, const basic_vector<T> & q1, const basic_vector<T> & q2,
	bool & degenerate, T & h, T b[3])
{
	basic_vector<T> qq1 = q1 - q0, qq2 = q2 - q0, pq = p - q0;
	// Face normal
	basic_vector<T> n = outer_product(qq1, qq2);
	T n2 = square(n);
	T eps = std::numeric_limits<T>::epsilon();

	if (degenerate = (n2 < eps * eps))
		return false;

	h = scalar_product(pq, n) / sqrt(n2);

	b[1] = mixed_product(pq, qq2, n) / n2;
	if (fabs(b[1]) < 2 * eps)
		b[1] = 0.0;
	b[2] = mixed_product(qq1, pq, n) / n2;
	if (fabs(b[2]) < 2 * eps)
		b[2] = 0.0;
	b[0] = 1 - b[1] - b[2];

	return (0 <= b[0] && b[0] <= 1)
		&& (0 <= b[1] && b[1] <= 1)
		&& (0 <= b[2] && b[2] <= 1);
}

template <typename T>
bool intersect_segment_and_triangle(const basic_vector<T> & p0, const basic_vector<T> & p1,
	const basic_vector<T> & q0, const basic_vector<T> & q1, const basic_vector<T> & q2,
	bool & degenerate, T b[3], T & t)
{
	basic_vector<T> qq1 = q1 - q0, qq2 = q2 - q0, pq0 = p0 - q0, pq1 = p1 - q0;
	// Face normal
	basic_vector<T> n = qq1 ^ qq2;
	T n2 = square(n);
	T eps = std::numeric_limits<T>::epsilon();
	// The face is degenerate?
	if (degenerate = (n2 < eps * eps))
		return false;
	T absn = sqrt(n2);
	T h0 = scalar_product(pq0, n) / absn;
	T h1 = scalar_product(pq1, n) / absn;
	if ((h0 > 0 && h1 > 0) || (h0 < 0 && h1 < 0))
		return false;
	T h = h0 - h1;
	// The segment is too short or nearly coplanar to the face?
	if (degenerate = (fabs(h) < EPSILON))
		return false;
	t = h0 / h;
	// Projection point
	basic_vector<T> p = p0 + (p1 - p0) * t, pq = p - q0;
	
	T test = pq * n;
	assert(fabs(test) < 24 * eps); // Mysterious constant!!!

	b[1] = mixed_product(pq, qq2, n) / n2;
	b[2] = mixed_product(qq1, pq, n) / n2;
	b[0] = 1 - b[1] - b[2];

	return (0 <= b[0] && b[0] <= 1)
		&& (0 <= b[1] && b[1] <= 1)
		&& (0 <= b[2] && b[2] <= 1);
}

template <typename T>
T distance_to_segment(const basic_vector<T> & p, 
	const basic_vector<T> & p0, const basic_vector<T> & p1, 
	bool & degenerate, T & h2, T & t)
{
	if (project_point_on_segment(p, p0, p1, degenerate, h2, t))
		return sqrt(h2);
	else 
	{
		// Return distance to one of endpoints
		T d0 = abs(p0 - p), d1 = abs(p1 - p);
		if (d1 < d0)
		{
			t = 1;
			return d1;
		}
		else
		{
			t = 0;
			return d0;
		}		
	}
}


template <typename T>
T distance_to_triangle(const basic_vector<T> & p,
	const basic_vector<T> & q0, const basic_vector<T> & q1, const basic_vector<T> & q2,
	bool & degenerate, T & h, T b[3])
{
	if (project_point_on_triangle(p, q0, q1, q2, degenerate, h, b))
		return fabs(h);
	else
		if (!degenerate)
		{
			// Just fall outside the triangle
			if (b[0] <= 0)
			{
				if (b[1] <= 0)
				{
					// Return distance to q2
					b[0] = 0; b[1] = 0; b[2] = 1.0;
					return abs(p - q2);
				}
				else if (b[2] <= 0)
				{
					// Return distance to q1
					b[0] = 0; b[1] = 1.0; b[2] = 0;
					return abs(p - q1);
				}
				else // b[1] > 0 && b[2] > 0
				{
					T h2, t;
					T d = distance_to_segment(p, q1, q2, degenerate, h2, t);
					assert(!degenerate);
					b[0] = 0; b[1] = (1 - t); b[2] = t;
					return d;
				}
			}
			else if (b[1] <= 0)
			{
				if (b[2] <= 0)
				{
					// Return distance to q0
					b[0] = 1.0; b[1] = 0; b[2] = 0;
					return abs(p - q0);
				}
				else // b[0] > 0 && b[2] > 0
				{
					T h2, t;
					T d = distance_to_segment(p, q0, q2, degenerate, h2, t);
					assert(!degenerate);
					b[0] = (1 - t); b[1] = 0; b[2] = t;
					return d;
				}
			}
			else // b[0] > 0 && b[1] > 0
			{
				T h2, t;
				T d = distance_to_segment(p, q0, q1, degenerate, h2, t);
				assert(!degenerate);
				b[0] = (1 - t); b[1] = t; b[2] = 0;
				return d;
			}
		}
		else // degenerate
		{
			// Return distance to the longest edge
			T l0 = abs(q1 - q0), l1 = abs(q2 - q1), l2 = abs(q0 - q2);
			if (l0 <= l1)
			{
				if (l1 <= l2)
				{
					// l0 <= l1 <= l2
					T h2, t;
					T d = distance_to_segment(p, q0, q2, degenerate, h2, t);
					b[0] = (1 - t); b[1] = 0; b[2] = t;
					return d;
				}
				else
				{
					// l0 <= l2 < l1
					T h2, t;
					T d = distance_to_segment(p, q1, q2, degenerate, h2, t);
					b[0] = 0; b[1] = (1 - t); b[2] = t;
					return d;
				}
			}
			else
			{
				if (l0 <= l2)
				{
					// l1 < l0 <= l2
					T h2, t;
					T d = distance_to_segment(p, q0, q2, degenerate, h2, t);
					b[0] = (1 - t); b[1] = 0; b[2] = t;
					return d;
				}
				else
				{
					// l1 < l2 < l0
					T h2, t;
					T d = distance_to_segment(p, q0, q1, degenerate, h2, t);
					b[0] = (1 - t); b[1] = t; b[2] = 0;
					return d;
				}
			}
		}
}


template <typename T>
void get_vertex_indices(
	const std::vector< basic_vector<T> > & S1, 
	const hash_grid<int> & grid, 
	const std::vector< basic_vector<T> > & S2, 
	std::vector<int> & indices, // output
	T tolerance,
	bool required)
{
	indices.clear();
	indices.resize(S2.size());
	for (size_t i = 0; i < S2.size(); ++i)
	{
		const basic_vector<T> & p = S2[i];
		box b(p, p);
		b.inflate(tolerance);
		T dmin = tolerance; // distance
		int index = -1;
		hash_grid<int>::range_iterator rit;
		for (rit = grid.range_begin(b); !rit.end(); ++rit) 
		{
			int j = * rit;
			T d = abs(S1.at(j) - p);
			if (d < dmin) {
				dmin = d;
				index = j;
			}
		}
		if (required && index == -1)
		{
			std::ostringstream out;
			out << "Failed to map vertex #" << i;
			throw std::runtime_error(out.str());
		}
		indices[i] = index; // may be -1
	}
}

}

